using RobotSimulator.Basic;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class FRGPSController : RobotController
{
    private FourRotorController target;
    public Vector2 target_xy;
    public Vector3 pid_x_k;
    public Vector3 pid_y_k;
    private Pid pid_x;
    private Pid pid_y;
    private GPS gps;
    protected override void Start()
    {  
        base.Start();
        pid_x=Pid.AddPid(this.gameObject, pid_x_k);
        pid_y=Pid.AddPid(this.gameObject, pid_y_k);
    }
    protected override void SetUp()
    {
        base.SetUp();
        target = (FourRotorController)this.control_target;
        gps = ((FourRotor)target.control_target).gps;
        //target.beforeControl = control;
    }
    public void control()
    {
        gps.RecvCmd(new CmdGetGPS());
        if (gps.output_data != "")
        {
            Vector2 pos = JsonUtility.FromJson<Vector2>(gps.output_data);
            var x_output = -pid_x.Control(target_xy.x - pos.x);
            var y_output = pid_y.Control(target_xy.y - pos.y);
            if (x_output > 10)
            {
                x_output = 10;
            }else if (x_output < -10)
            {
                x_output = -10;
            }
            if (y_output > 10)
            {
                y_output = 10;
            }
            else if (y_output < -10)
            {
                y_output = -10;
            }
            var l=Mathf.Sqrt(x_output*x_output + y_output*y_output);
            var yaw = Mathf.Atan2(y_output,x_output)- target.yaw_angle*Mathf.Deg2Rad;
            target.target_rotation.x = Mathf.Cos(yaw) * l;
            target.target_rotation.z = Mathf.Sin(yaw) * l;
        } 
    }
    protected override void Loop()
    {
        if (contorl_flag)
        {
            target.beforeControl = control;
        }
        else
        {
            target.beforeControl = (() => { });
        }
       
    }


}
